﻿// #include <iostream>
// #include <thread>
// #include "test_pcl.h"

// using namespace std::chrono_literals;

// pcl::visualization::PCLVisualizer::Ptr
// simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
// {
// 	// --------------------------------------------
// 	// -----Open 3D viewer and add point cloud-----
// 	// --------------------------------------------
// 	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
// 	viewer->setBackgroundColor(0, 0, 0);
// 	viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
// 	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
// 	//viewer->addCoordinateSystem (1.0, "global");
// 	viewer->initCameraParameters();
// 	return (viewer);
// }

// int
// main(int argc, char** argv)
// {
// 	// initialize PointClouds
	// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

	// // populate our PointCloud with points
	// cloud->width = 500;
	// cloud->height = 1;
	// cloud->is_dense = false;
	// cloud->points.resize(cloud->width * cloud->height);
	// for (pcl::index_t i = 0; i < static_cast<pcl::index_t>(cloud->size()); ++i)
	// {
	// 	if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
	// 	{
	// 		(*cloud)[i].x = 1024 * rand() / (RAND_MAX + 1.0);
	// 		(*cloud)[i].y = 1024 * rand() / (RAND_MAX + 1.0);
	// 		if (i % 5 == 0)
	// 			(*cloud)[i].z = 1024 * rand() / (RAND_MAX + 1.0);
	// 		else if (i % 2 == 0)
	// 			(*cloud)[i].z = sqrt(1 - ((*cloud)[i].x * (*cloud)[i].x)
	// 				- ((*cloud)[i].y * (*cloud)[i].y));
	// 		else
	// 			(*cloud)[i].z = -sqrt(1 - ((*cloud)[i].x * (*cloud)[i].x)
	// 				- ((*cloud)[i].y * (*cloud)[i].y));
	// 	}
	// 	else
	// 	{
	// 		(*cloud)[i].x = 1024 * rand() / (RAND_MAX + 1.0);
	// 		(*cloud)[i].y = 1024 * rand() / (RAND_MAX + 1.0);
	// 		if (i % 2 == 0)
	// 			(*cloud)[i].z = 1024 * rand() / (RAND_MAX + 1.0);
	// 		else
	// 			(*cloud)[i].z = -1 * ((*cloud)[i].x + (*cloud)[i].y);
	// 	}
	// }

// 	std::vector<int> inliers;

// 	// created RandomSampleConsensus object and compute the appropriated model
// 	pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
// 		model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
// 	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
// 		model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
// 	if (pcl::console::find_argument(argc, argv, "-f") >= 0)
// 	{
// 		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
// 		ransac.setDistanceThreshold(.01);
// 		ransac.computeModel();
// 		ransac.getInliers(inliers);
// 	}
// 	else if (pcl::console::find_argument(argc, argv, "-sf") >= 0)
// 	{
// 		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
// 		ransac.setDistanceThreshold(.01);
// 		ransac.computeModel();
// 		ransac.getInliers(inliers);
// 	}

// 	// copies all inliers of the model computed to another PointCloud
// 	pcl::copyPointCloud(*cloud, inliers, *final);

// 	// creates the visualization object and adds either our original cloud or all of the inliers
// 	// depending on the command line arguments specified.
// 	pcl::visualization::PCLVisualizer::Ptr viewer;
// 	if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
// 		viewer = simpleVis(final);
// 	else
// 		viewer = simpleVis(cloud);
// 	while (!viewer->wasStopped())
// 	{
// 		viewer->spinOnce(100);
// 		std::this_thread::sleep_for(std::chrono::milliseconds(100));
// 	}
// 	return 0;
// }

